from sls_controller.sls_missionsABC import Missions,MissionsState
import math
import numpy as np
import os
import csv
from libs.sls_mirrors.sls_agent import Mirror
from libs.sls_mirrors.sls_UGVmirrorsUpdate import UGVBasicPositionMirrorUpdate

class MissionsPurePursuitMuti(Missions):
    def __init__(self,publicResource,datasets:dict={"latitudes":[47.3978,47.3878],"longitudes":[8.5461,8.5361],"boundaries":{"latitudes":[],"longitudes":[]}}):
        super().__init__(publicResource, datasets)
        #trans to localPositions in local NED frames
        self.localPositions = []
        self.trans_to_localPositions()
        self.number_of_points = len(self.localPositions)
        self.dest_index = 0
        #
        self.z_P = 0.5
        self.max_vel_z = 1
        self.max_vel_xy = 10
        self.min_vel_xy = 1
        self.dest_x = 0.0
        self.dest_y = 0.0
        self.dest_angle = 0.0
        self.delta_T = 0.0
        self.accpet_radius = 1
        self.point_accpet_radius = self.accpet_radius
    def fore_check(self):
        if self.foreCheckTimes == -1:         
            if self.publicResource.uav_resource.is_arming_status_armed():
                return True
            else:
                self.publicResource.uav_resource.set_arm()
                return False

    def done_check(self):
        if self.dest_index == self.number_of_points:
            return True
        else:
            return False
        
    def mission_action(self):
        #control altitude using p controller
        vel_z = -(self.defualt_altitude - self.publicResource.uav_resource.get_msl_altitude())*self.z_P
        #clamping
        vel_z = max(min(vel_z,self.max_vel_z),-self.max_vel_z)
        #control xy
        #if dest is reach
        if math.dist([self.now_x,self.now_y],[self.dest_x,self.dest_y])<self.accpet_radius:
            self.dest_index += 1
            if self.dest_index == self.number_of_points:
                return
        self.now_x = self.publicResource.uav_resource.get_NED_position()["N"]
        self.now_y = self.publicResource.uav_resource.get_NED_position()["E"]
        self.dest_x = self.localPositions[self.dest_index][0]
        self.dest_y = self.localPositions[self.dest_index][1]

        xySpeed,xySpeedAngle = self.publicResource.tools_resource.claculate_distanceAndcourseAngle(self.now_x,self.now_y,self.dest_x,self.dest_y)

        xySpeed = self.speed_constraint(xySpeed)
        xSpeed = xySpeed * math.cos(xySpeedAngle)
        ySpeed = xySpeed * math.sin(xySpeedAngle)
        speed_now_x = self.publicResource.uav_resource.get_NED_velocity()["N"]
        speed_now_y = self.publicResource.uav_resource.get_NED_velocity()["E"]
        xySpeed = self.get_speed_profile(speed_now_x,speed_now_y,xSpeed,ySpeed)
        xSpeed = xySpeed * math.cos(xySpeedAngle)
        ySpeed = xySpeed * math.sin(xySpeedAngle)       
        mid_x = xySpeed * math.cos(xySpeedAngle)*self.delta_T +  self.now_x
        mid_y = xySpeed * math.sin(xySpeedAngle)*self.delta_T +  self.now_y
            

        #send tra
        self.publicResource.uav_resource.platform_interface.publish_trajectory_setpoint(position=[mid_x,mid_y,None],velocity=[xSpeed,ySpeed,vel_z])


        pass
    def mission_enter(self):
        #record relatvie  altitude
        self.defualt_altitude = self.publicResource.uav_resource.get_msl_altitude()
        self.delta_T = self.publicResource.get_main_delta_time()

    def mission_exit(self):
        pass

    def abort_check(self):
        pass
    
    def mission_abort(self):
        pass
    
    def speed_constraint(self,speed):
        return min(speed,self.max_vel_xy)
    
    def get_speed_profile(self,now_speed_x,now_speed_y,speed_x,speed_y,sigma = 1):
        now_speed = np.array([now_speed_x,now_speed_y])
        speed = np.array([speed_x,speed_y])

        dot_product = np.dot(now_speed,speed)
        norm_product = np.linalg.norm(now_speed)*np.linalg.norm(speed)
        cos_theta = np.clip(dot_product/norm_product, -1.0, 1.0)
        theta_rad = np.arccos(cos_theta)
        return self.min_vel_xy + (self.max_vel_xy-self.min_vel_xy)*np.exp((-theta_rad**2)/(sigma**2))


    def trans_to_localPositions(self):
        for i in range(len(self.datasets["latitudes"])):
            self.localPositions.append(self.publicResource.tools_resource.azimuthal_projection(self.datasets["latitudes"][i],self.datasets["longitudes"][i],self.publicResource.uav_resource.get_local_ref_lat(),self.publicResource.uav_resource.get_local_ref_lon()))

class MissionsPurePursuit(Missions):
    def __init__(self,publicResource,datasets:dict={"latitude":47.3978,"longitude":8.5461}):
        super().__init__(publicResource,datasets)
        self.z_P = 0.5
        self.max_vel_z = 1
        self.max_vel_xy = 10
        self.min_vel_xy = 1
        self.dest_x = 0.0
        self.dest_y = 0.0
        self.dest_angle = 0.0
        self.delta_T = 0.0
        self.accpet_radius = 1
    def fore_check(self):
        if self.foreCheckTimes == -1:         
            if self.publicResource.uav_resource.is_arming_status_armed():
                return True
            else:
                self.publicResource.uav_resource.set_arm()
                return False

    def done_check(self):
        if math.dist([self.now_x,self.now_y],[self.dest_x,self.dest_y])<self.accpet_radius:
            return True
        else:
            return False
        
    def mission_action(self):
        #control altitude using p controller
        vel_z = -(self.defualt_altitude - self.publicResource.uav_resource.get_msl_altitude())*self.z_P
        #clamping
        vel_z = max(min(vel_z,self.max_vel_z),-self.max_vel_z)
        #control xy
        self.now_x = self.publicResource.uav_resource.get_NED_position()["N"]
        self.now_y = self.publicResource.uav_resource.get_NED_position()["E"]
        xySpeed,xySpeedAngle = self.publicResource.tools_resource.claculate_distanceAndcourseAngle(self.now_x,self.now_y,self.dest_x,self.dest_y)
        xySpeed = self.speed_constraint(xySpeed)
        xSpeed = xySpeed * math.cos(xySpeedAngle)
        ySpeed = xySpeed * math.sin(xySpeedAngle)
        speed_now_x = self.publicResource.uav_resource.get_NED_velocity()["N"]
        speed_now_y = self.publicResource.uav_resource.get_NED_velocity()["E"]
        xySpeed = self.get_speed_profile(speed_now_x,speed_now_y,xSpeed,ySpeed)

        mid_x = xySpeed * math.cos(xySpeedAngle)*self.delta_T +  self.now_x
        mid_y = xySpeed * math.sin(xySpeedAngle)*self.delta_T +  self.now_y
        #send tra
        self.publicResource.uav_resource.platform_interface.publish_trajectory_setpoint(position=[mid_x,mid_y,None],velocity=[xSpeed,ySpeed,vel_z])


        pass
    def mission_enter(self):
        #record relatvie  altitude
        self.defualt_altitude = self.publicResource.uav_resource.get_msl_altitude()
        self.dest_x,self.dest_y,self.dest_angle = self.publicResource.tools_resource.azimuthal_projection(self.datasets["latitude"],self.datasets["longitude"],self.publicResource.uav_resource.get_local_ref_lat(),self.publicResource.uav_resource.get_local_ref_lon())
        self.delta_T = self.publicResource.get_main_delta_time()

    def mission_exit(self):
        pass

    def abort_check(self):
        pass
    
    def mission_abort(self):
        pass
    
    def speed_constraint(self,speed):
        return min(speed,self.max_vel_xy)
    
    def get_speed_profile(self,now_speed_x,now_speed_y,speed_x,speed_y,sigma = 1):
        now_speed = np.array([now_speed_x,now_speed_y])
        speed = np.array([speed_x,speed_y])

        dot_product = np.dot(now_speed,speed)
        norm_product = np.linalg.norm(now_speed)*np.linalg.norm(speed)
        cos_theta = np.clip(dot_product/norm_product, -1.0, 1.0)
        theta_rad = np.arccos(cos_theta)
        return self.min_vel_xy + (self.max_vel_xy-self.min_vel_xy)*np.exp((-theta_rad**2)/(sigma**2))    
    


class MissionsEBLOSSteering(Missions):
    def __init__(self,publicResource,datasets:dict={"latitudes":[47.3978,47.3878],"longitudes":[8.5461,8.5361],"boundaries":{"latitudes":[],"longitudes":[]},"rel_alt":5,}):
        super().__init__(publicResource, datasets)
        #trans to localPositions in local NED frames
        self.localPositions = []
        self.trans_to_localPositions()
        self.number_of_points = len(self.localPositions)
        self.dest_index = 0
        #
        self.z_P = 0.5
        self.max_vel_z = 1
        self.max_vel_xy = 10
        self.min_vel_xy = 1
        self.dest_x = 0.0
        self.dest_y = 0.0
        self.dest_angle = 0.0
        self.delta_T = 0.0
        self.accpet_radius = 1
        self.point_accpet_radius = self.accpet_radius
        #
        self.set_NED_alt = None



    def fore_check(self):
        if self.foreCheckTimes == -1:         
            if self.publicResource.uav_resource.is_arming_status_armed():
                return True
            else:
                self.publicResource.uav_resource.set_arm()
                return False

    def done_check(self):
        if self.dest_index == self.number_of_points:
            return True
        else:
            return False
        
    def mission_action(self):
        #control xy
        self.now_x = self.publicResource.uav_resource.get_NED_position()["N"]
        self.now_y = self.publicResource.uav_resource.get_NED_position()["E"]
        self.dest_x = self.localPositions[self.dest_index][0]
        self.dest_y = self.localPositions[self.dest_index][1]
        #if dest is reach
        if math.dist([self.now_x,self.now_y],[self.dest_x,self.dest_y])<self.accpet_radius:
            self.dest_index += 1
            if self.dest_index == self.number_of_points:
                return
            else:
                self.dest_x = self.localPositions[self.dest_index][0]
                self.dest_y = self.localPositions[self.dest_index][1]
        #if dest_index is 0 using Pure Pursuit
        if self.dest_index == 0:
            xySpeed,xySpeedAngle = self.publicResource.tools_resource.claculate_distanceAndcourseAngle(self.now_x,self.now_y,self.dest_x,self.dest_y)
        else:
            last_x = self.localPositions[self.dest_index-1][0]
            last_y = self.localPositions[self.dest_index-1][1]
            los_x,los_y = self.get_los_setpoints(last_x,last_y,self.dest_x,self.dest_y,self.now_x,self.now_y,point_accept_radius=self.point_accpet_radius)            
            xySpeed,xySpeedAngle = self.publicResource.tools_resource.claculate_distanceAndcourseAngle(self.now_x,self.now_y,los_x,los_y)

        xySpeed = self.speed_constraint(xySpeed)
        xSpeed = xySpeed * math.cos(xySpeedAngle)
        ySpeed = xySpeed * math.sin(xySpeedAngle)
        # speed_now_x = self.publicResource.uav_resource.get_NED_velocity()["N"]
        # speed_now_y = self.publicResource.uav_resource.get_NED_velocity()["E"]
        # if self.dest_index == 0:
        #     xySpeed = self.get_speed_profile(speed_now_x,speed_now_y,xSpeed,ySpeed)
        # else:
        #     xySpeed = self.get_speed_profile(xSpeed,ySpeed,self.dest_x-los_x,self.dest_y-los_y,sigma=0.1)
        # xSpeed = xySpeed * math.cos(xySpeedAngle)
        # ySpeed = xySpeed * math.sin(xySpeedAngle)       
        mid_x = xySpeed * math.cos(xySpeedAngle)*self.delta_T +  self.now_x
        mid_y = xySpeed * math.sin(xySpeedAngle)*self.delta_T +  self.now_y
            

        if self.set_NED_alt == None:
            #control altitude using p controller
            vel_z = -(self.defualt_altitude - self.publicResource.uav_resource.get_msl_altitude())*self.z_P
            #clamping
            vel_z = max(min(vel_z,self.max_vel_z),-self.max_vel_z)        
            #send tra
            self.publicResource.uav_resource.platform_interface.publish_trajectory_setpoint(position=[mid_x,mid_y,None],velocity=[xSpeed,ySpeed,vel_z])
        else:
            self.publicResource.uav_resource.platform_interface.publish_trajectory_setpoint(position=[mid_x,mid_y,self.set_NED_alt],velocity=[xSpeed,ySpeed,0])


        pass
    def mission_enter(self):
        #record relatvie  altitude
        self.defualt_altitude = self.publicResource.uav_resource.get_msl_altitude()
        self.delta_T = self.publicResource.get_main_delta_time()
        #has rel_alt using ned ,else using msl
        if "rel_alt" in self.datasets:
            if self.datasets["rel_alt"] != None:
                self.set_NED_alt = -self.datasets["rel_alt"]+self.publicResource.uav_resource.get_takeoff_NED_position()["D"]
        else:
            self.set_NED_alt = None

    def mission_exit(self):
        pass

    def abort_check(self):
        pass
    
    def mission_abort(self):
        pass
    
    def speed_constraint(self,speed):
        return min(speed,self.max_vel_xy)
    
    def get_speed_profile(self,now_speed_x,now_speed_y,speed_x,speed_y,sigma = 1):
        now_speed = np.array([now_speed_x,now_speed_y])
        speed = np.array([speed_x,speed_y])

        dot_product = np.dot(now_speed,speed)
        norm_product = np.linalg.norm(now_speed)*np.linalg.norm(speed)
        cos_theta = np.clip(dot_product/norm_product, -1.0, 1.0)
        theta_rad = np.arccos(cos_theta)
        return self.min_vel_xy + (self.max_vel_xy-self.min_vel_xy)*np.exp((-theta_rad**2)/(sigma**2))


    def trans_to_localPositions(self):
        for i in range(len(self.datasets["latitudes"])):
            self.localPositions.append(self.publicResource.tools_resource.azimuthal_projection(self.datasets["latitudes"][i],self.datasets["longitudes"][i],self.publicResource.uav_resource.get_local_ref_lat(),self.publicResource.uav_resource.get_local_ref_lon()))


    def get_los_setpoints(self,last_x,last_y,dest_x,dest_y,now_x,now_y,point_accept_radius,method='enclosure',dist_cs_function = None):
        dy = dest_y - last_y
        dx = dest_x - last_x
        if dx == 0:
            cross_y = now_y
            cross_x = last_x
        else:
            k = dy/dx
            cross_x = (now_x+math.pow(k,2)*last_x+k*now_y-k*last_y)/(1+math.pow(k,2))
            cross_y = last_y+k*(cross_x-last_x)

        ang = (last_y - cross_y)*(dest_y-cross_y)+(last_x - cross_x)*(dest_x-cross_x)
        dist_cn = math.dist((now_x,now_y),(cross_x,cross_y))
        if method == 'enclosure':
        #enclosure_based LOS Steering
        #判断现在距离垂直点的距离是否大于点接受半径
            if dist_cn >= point_accept_radius:
                los_x = cross_x
                los_y = cross_y
            else:
                dist_cs = math.sqrt(math.pow(point_accept_radius,2) - math.pow(dist_cn,2))
                head_x = dest_x - cross_x
                head_y = dest_y - cross_y
                los_x = cross_x + dist_cs*head_x/math.sqrt(math.pow(head_y,2)+math.pow(head_x,2))
                los_y = cross_y + dist_cs*head_y/math.sqrt(math.pow(head_y,2)+math.pow(head_x,2))
        elif method == 'lookahead':
        #lookahead_based LOS Steering
            dist_cs = dist_cs_function(dist_cn)
            los_x = cross_x + dist_cs*dx/math.sqrt(math.pow(dy,2)+math.pow(dx,2))
            los_y = cross_y + dist_cs*dy/math.sqrt(math.pow(dy,2)+math.pow(dx,2))
        return los_x,los_y

class UGVPurePursuit(Missions):
    def __init__(self,publicResource,datasets:dict={}):
        super().__init__(publicResource,datasets)
        self.z_P = 0.5
        self.max_vel_z = 1
        self.max_vel_xy = 10
        self.min_vel_xy = 1
        self.dest_x = 0.0
        self.dest_y = 0.0
        self.dest_angle = 0.0
        self.delta_T = 0.0
        self.accpet_radius = 1
    def fore_check(self):
        if self.foreCheckTimes == -1:         
            if self.publicResource.uav_resource.is_arming_status_armed():
                return True
            else:
                self.publicResource.uav_resource.set_arm()
                return False

    def done_check(self):
        return False
        
    def mission_action(self):
        #control altitude using p controller
        vel_z = -(self.defualt_altitude - self.publicResource.uav_resource.get_msl_altitude())*self.z_P
        #clamping
        vel_z = max(min(vel_z,self.max_vel_z),-self.max_vel_z)
        #control xy
        self.now_x = self.publicResource.uav_resource.get_NED_position()["N"]
        self.now_y = self.publicResource.uav_resource.get_NED_position()["E"]
        xySpeed,xySpeedAngle = self.publicResource.tools_resource.claculate_distanceAndcourseAngle(self.now_x,self.now_y,self.dest_x,self.dest_y)
        xySpeed = self.speed_constraint(xySpeed)
        xSpeed = xySpeed * math.cos(xySpeedAngle)
        ySpeed = xySpeed * math.sin(xySpeedAngle)
        speed_now_x = self.publicResource.uav_resource.get_NED_velocity()["N"]
        speed_now_y = self.publicResource.uav_resource.get_NED_velocity()["E"]
        xySpeed = self.get_speed_profile(speed_now_x,speed_now_y,xSpeed,ySpeed)

        mid_x = xySpeed * math.cos(xySpeedAngle)*self.delta_T +  self.now_x
        mid_y = xySpeed * math.sin(xySpeedAngle)*self.delta_T +  self.now_y
        #send tra
        self.publicResource.uav_resource.platform_interface.publish_trajectory_setpoint(position=[mid_x,mid_y,None],velocity=[xSpeed,ySpeed,vel_z])


        pass
    def mission_enter(self):
        #record relatvie  altitude
        self.defualt_altitude = self.publicResource.uav_resource.get_msl_altitude()
        #step to use the mirror
        #1.create ugv mirror
        ugv_mirror = Mirror("ugv1")
        #2. add mirror updates to mirror which must before the mirror is added to agent
        ugv_mirror.add_mirror_updates(UGVBasicPositionMirrorUpdate)
        #3. add mirror to agent
        self.publicResource.mirrors_agent.add_mirrors(ugv_mirror)
        #4. add notify func to mirror update
        self.publicResource.mirrors_agent.add_notify_func_to_mirror_update("ugv1",UGVBasicPositionMirrorUpdate,self.ugv_position_update)

        self.delta_T = self.publicResource.get_main_delta_time()

    def mission_exit(self):
        pass

    def abort_check(self):
        pass
    
    def mission_abort(self):
        pass
    
    def speed_constraint(self,speed):
        return min(speed,self.max_vel_xy)
    
    def get_speed_profile(self,now_speed_x,now_speed_y,speed_x,speed_y,sigma = 1):
        now_speed = np.array([now_speed_x,now_speed_y])
        speed = np.array([speed_x,speed_y])

        dot_product = np.dot(now_speed,speed)
        norm_product = np.linalg.norm(now_speed)*np.linalg.norm(speed)
        cos_theta = np.clip(dot_product/norm_product, -1.0, 1.0)
        theta_rad = np.arccos(cos_theta)
        return self.min_vel_xy + (self.max_vel_xy-self.min_vel_xy)*np.exp((-theta_rad**2)/(sigma**2)) 
    
    def ugv_position_update(self,mirror,mirrorUpdate:UGVBasicPositionMirrorUpdate):
        self.ugv_position = mirrorUpdate.GNSS_position
        self.dest_x,self.dest_y,self.dest_angle = self.publicResource.tools_resource.azimuthal_projection(self.ugv_position[1],self.position[0],self.publicResource.uav_resource.get_local_ref_lat(),self.publicResource.uav_resource.get_local_ref_lon())



from geopy.distance import geodesic

class MissionsDroneDirectPursuitCar(Missions):
    def __init__(self,publicResource,datasets:dict=None):
        super().__init__(publicResource,datasets)
        self.ugv_longitude = 0.0
        self.ugv_latitude = 0.0
        self.ugv_yaw = 0.0
        self.ugv_iseffect = False

        self.temp_pos = None

        self.N_PID = self.PID()
        self.E_PID = self.PID()
        self.N_PID.set_PID(1,0.03,0,20,2)
        self.E_PID.set_PID(1,0.03,0,20,2)
        self.get_current_yaw = self.publicResource.uav_resource.get_vehicle_yaw

    def fore_check(self):
        if self.foreCheckTimes == -1:         
            if self.publicResource.uav_resource.is_arming_status_armed():
                return True
            else:
                self.publicResource.uav_resource.set_arm()
                return False

    def done_check(self):
        return False
        
    def mission_action(self):
        if self.ugv_iseffect:
            N, E = self.dGPS2dNE(
                                    GPS1=(
                                        self.publicResource.uav_resource.get_GNSS_position()["latitude"],
                                        self.publicResource.uav_resource.get_GNSS_position()["longitude"]
                                    ),
                                    GPS2=(self.ugv_latitude, self.ugv_longitude)
                                )
            N_velocity = self.N_PID.PID_caculate(N)
            E_velocity = self.E_PID.PID_caculate(E)
            self.publicResource.uav_resource.set_vehicle_velocity(N_velocity,E_velocity,0,None)
            self.temp_pos = None
            # print(f"N_velocity:{N_velocity}, E_velocity:{E_velocity}")
            # print(f"{self.publicResource.uav_resource.get_local_ref_lat()},{self.publicResource.uav_resource.get_local_ref_lon()}")
            # print({self.ugv_latitude}, {self.ugv_longitude})
        else:
            if self.temp_pos is None:
                self.temp_pos = self.publicResource.uav_resource.get_NED_position()
            else:
                self.publicResource.uav_resource.set_vehicle_position(
                    self.temp_pos["N"],self.temp_pos["E"],self.temp_pos["D"],self.get_current_yaw())
            print('no')
            # print(f"{self.publicResource.uav_resource.get_local_ref_lat()},{self.publicResource.uav_resource.get_local_ref_lon()}")
    def mission_enter(self):
        #step to use the mirror
        #1.create ugv mirror
        ugv_mirror = Mirror("ugv1")
        #2. add mirror updates to mirror which must before the mirror is added to agent
        ugv_mirror.add_mirror_updates(UGVBasicPositionMirrorUpdate)
        #3. add mirror to agent
        self.publicResource.mirrors_agent.add_mirrors(ugv_mirror)
        #4. add notify func to mirror update
        self.publicResource.mirrors_agent.add_notify_func_to_mirror_update("ugv1",UGVBasicPositionMirrorUpdate,self.ugv_position_update)


    def ugv_position_update(self,mirror,mirrorUpdate:UGVBasicPositionMirrorUpdate):
        self.ugv_position = mirrorUpdate.GNSS_position
        self.vehicle_yaw = mirrorUpdate.vehicle_yaw
        if self.ugv_position == None:
            self.ugv_latitude = -1.0
            self.ugv_longitude = -1.0
            self.vehicle_yaw = -1.0
            self.ugv_iseffect = False
        else:
            self.ugv_longitude = self.ugv_position[0]*1.0
            self.ugv_latitude = self.ugv_position[1]*1.0
            self.vehicle_yaw = self.vehicle_yaw*1.0
            self.ugv_iseffect = True 
        print(f"{self.ugv_latitude = },{self.ugv_longitude = },{self.vehicle_yaw = },{self.ugv_iseffect = }")

    def mission_exit(self):
        pass

    def abort_check(self):
        pass
    
    def mission_abort(self):
        pass

    def dGPS2dNE(self,GPS1: tuple, GPS2: tuple):
        """
        计算从 GPS1 到 GPS2 的相对 North-East 偏移（单位：米）
        仅使用纬度、经度（单位为度），忽略高程。
        输出为 NED 坐标中的 [North, East]，Down 默认为 0
        """
        lat1, lon1 = GPS1
        lat2, lon2 = GPS2

        # North 分量：固定经度，移动纬度
        north_point = (lat2, lon1)
        north = geodesic((lat1, lon1), north_point).meters
        if lat2 < lat1:
            north = -north

        # East 分量：固定纬度，移动经度
        east_point = (lat1, lon2)
        east = geodesic((lat1, lon1), east_point).meters
        if lon2 < lon1:
            east = -east
        return north, east
    
    class PID:
        def __init__(self):
            self.kp = 0.0
            self.ki = 0.0
            self.kd = 0.0
            self.prev_err = 0.0
            self.integral = 0.0
            self.integral_limit = 10.0
            self.output_limit = 1.0

        def set_PID(self, kp:float=0.0, ki:float=0.0, kd:float=0.0, integrate_limit:float=10, output_limit:float=1):
            self.kp = kp
            self.ki = ki
            self.kd = kd
            self.integral_limit = integrate_limit
            self.output_limit = output_limit

        def PID_caculate(self, err:float=0.0):
            self.integral += err
            if self.integral > self.integral_limit:
                self.integral = self.integral_limit
            if self.integral < -self.integral_limit:
                self.integral = -self.integral_limit
            output = self.kp*err + self.ki*self.integral + self.kd*(err - self.prev_err)
            if output > self.output_limit:
                output = self.output_limit
            if output < -self.output_limit:
                output = -self.output_limit
            self.prev_err = err
            return output
        

class MissionsDroneGsta(Missions):
    def __init__(self, publicResource, datasets={"latitudes": [47.3978, 47.3878], "longitudes": [8.5461, 8.5361], "boundaries": {"latitudes": [], "longitudes": []}, "rel_alt": 5}):
        super().__init__(publicResource, datasets)
        self.localPositions = []
        self.localPosition = []
        self.trans_to_localPositions()
        self.number_of_points = len(self.localPositions)
        self.dest_index = 0
        self.z_P = 0.5
        self.max_vel_z = 1
        self.max_vel_xy = 5.0
        self.min_vel_xy = 1
        self.dest_x = 0.0
        self.dest_y = 0.0
        self.delta_T = 0.0
        self.a = 2
        self.t = 0
        self.set_NED_alt = None
        self.point_accept_radius = 0.5
        self.target_distances = []
        self.gsta_alpha = 0.7 #0.5-1
        self.gsta_beta = 2 * self.gsta_alpha - 1
        self.gsta_lam1 = 1.5
        self.gsta_lam2 = 2.0
        self.gsta_eps = 0.01
        self.max_side_speed = 5.0
        # --- 新增：误差记录 ---
        self.errors = []           # 横向误差列表
        self.times = []            # 时间戳
        self.current_error = []
        self.current_time = []
        # --- 新增：完整轨迹记录 ---
        self.actual_trajectory = []  # [(t, x, y), ...]
        self.planned_path = []       # 保存规划路径点 [(x, y), ...]

    def fore_check(self):
        if self.foreCheckTimes == -1:         
            if self.publicResource.uav_resource.is_arming_status_armed():
                return True
            else:
                self.publicResource.uav_resource.set_arm()
                return False

    def done_check(self):
        if self.dest_index == self.number_of_points:
            return True
        else:
            return False

    def mission_action(self):
        self.delta_T = self.publicResource.get_main_delta_time()
        self.now_x = self.publicResource.uav_resource.get_NED_position()["N"]
        self.now_y = self.publicResource.uav_resource.get_NED_position()["E"]

        if self.done_check():
            return
        self.dest_x, self.dest_y = self.localPosition[self.dest_index]

        if math.dist([self.now_x, self.now_y], [self.dest_x, self.dest_y]) < self.point_accept_radius:
            self.dest_index += 1
            if not hasattr(self, 'gsta_nu'):
                self.gsta_nu = 0.0
            if self.done_check():
                return
            self.dest_x, self.dest_y = self.localPosition[self.dest_index]
            if len(self.current_error) > 0:
                self.errors.append(self.current_error.copy())
                self.times.append(self.current_time.copy())

            # === 初始化下一段 ===
            self.current_error = []
            self.current_time = []
            self.t = 0

        last_x, last_y = self.localPosition[self.dest_index - 1]
        self.dest_x, self.dest_y = self.localPosition[self.dest_index]
        los_x, los_y, cross_x, cross_y = self.get_los_setpoints(last_x, last_y, self.dest_x, self.dest_y, self.now_x, self.now_y, point_accept_radius=20*self.point_accept_radius)
        seg_dx , seg_dy = self.dest_x - last_x, self.dest_y - last_y
        seg_dist = math.hypot(seg_dx, seg_dy)
        t_hat_x, t_hat_y = seg_dx / (seg_dist + 1e-6), seg_dy / (seg_dist + 1e-6)
        n_hat_x, n_hat_y = -t_hat_y, t_hat_x
        s = (-self.now_x + cross_x) * n_hat_x + (-self.now_y + cross_y) * n_hat_y
        track_line_error = math.dist([self.now_x, self.now_y], [cross_x, cross_y])
        print(f"[True Track Line Error] {track_line_error:.2f} m")
        if not hasattr(self, 'prev_x'):
            self.prev_x = self.now_x
            self.prev_y = self.now_y
            v_perp = 0.0
        else:
            vx = (self.now_x - self.prev_x) / self.delta_T
            vy = (self.now_y - self.prev_y) / self.delta_T
            v_perp = vx * n_hat_x + vy * n_hat_y
            self.prev_x = self.now_x
            self.prev_y = self.now_y
  

        s_pow_a = self.soft_pow(s, self.gsta_alpha, self.gsta_eps)
        s_pow_b = self.soft_pow(s, self.gsta_beta, self.gsta_eps)
        s_sign = self.soft_sign(s, self.gsta_eps)
        damping_gain = 1.5

        if self.dest_index == 1:
            u_perp = self.gsta_lam1 * s_pow_a *s_sign  - damping_gain * v_perp
        else:
            u_perp = self.gsta_lam1 * s_pow_a *s_sign + self.gsta_nu - damping_gain * v_perp
            self.gsta_nu += self.gsta_lam2 * s_pow_b * s_sign * self.delta_T
            self.gsta_nu = max(min(self.gsta_nu, 2.0), -2.0)
        u_perp = max(min(u_perp, self.max_side_speed), -self.max_side_speed)
        vx_cmd = u_perp * n_hat_x
        vy_cmd = u_perp * n_hat_y
        total_dist = self.target_distances[self.dest_index - 1]
        forward_speed = self.get_xyspeed_with_total_distance(total_dist)
        forward_angle = math.atan2(los_y - self.now_y , los_x - self.now_x)
        xSpeed = vx_cmd + forward_speed * math.cos(forward_angle)
        ySpeed = vy_cmd + forward_speed * math.sin(forward_angle)           
        speed_mag = math.hypot(xSpeed, ySpeed)
        speed_mag = self.speed_constraint(speed_mag)
        if speed_mag > 0:
            factor = speed_mag / (math.hypot(xSpeed, ySpeed) + 1e-6)
            xSpeed *= factor
            ySpeed *= factor
        else:
            xSpeed, ySpeed = 0.0, 0.0



        mid_x = self.now_x + xSpeed * self.delta_T
        mid_y = self.now_y + ySpeed * self.delta_T

        if self.set_NED_alt is None:
            vel_z = -(self.default_altitude - self.publicResource.uav_resource.get_msl_altitude()) * self.z_P
            vel_z = max(min(vel_z, self.max_vel_z), -self.max_vel_z)
            self.publicResource.uav_resource.platform_interface.publish_trajectory_setpoint(
                position=[mid_x, mid_y, None], velocity=[xSpeed, ySpeed, vel_z])
        else:
            self.publicResource.uav_resource.platform_interface.publish_trajectory_setpoint(
                position=[mid_x, mid_y, self.set_NED_alt], velocity=[xSpeed, ySpeed, 0])
         # ==记录每一段error和time==
        self.current_error.append({
            'error': track_line_error})
        self.current_time.append(self.t)
        # === 记录实际轨迹和风速 ===
        current_time = self.t  # 或使用 time.time() - mission_start_time，但你用 self.t 作为仿真时间
        self.actual_trajectory.append((current_time, self.now_x, self.now_y))
    
    def mission_enter(self):
        self.gsta_nu = 0.0 
        self.t = 0.0
        self.default_altitude = self.publicResource.uav_resource.get_msl_altitude()
        self.delta_T = self.publicResource.get_main_delta_time()
        if "rel_alt" in self.datasets and self.datasets["rel_alt"] is not None:
            self.set_NED_alt = -self.datasets["rel_alt"] + self.publicResource.uav_resource.get_takeoff_NED_position()["D"]
        else:
            self.set_NED_alt = None
        init_N = self.publicResource.uav_resource.get_NED_position()["N"]
        init_E = self.publicResource.uav_resource.get_NED_position()["E"]
        self.localPosition.insert(0, (init_N, init_E))
        self.localPositions.insert(0, (init_N, init_E))
        self.number_of_points = len(self.localPositions)
        self.compute_target_distances()
        # --- 初始化误差记录 ---
        self.errors = []
        self.times = []
        self.current_error = []
        self.current_time = []
        self.planned_path = self.localPosition[:]  # 复制一份

        # 初始化轨迹记录
        self.actual_trajectory = []

    
    def mission_exit(self):
        if len(self.current_error) > 0:
            self.errors.append(self.current_error.copy())
            self.times.append(self.current_time.copy())
        self.save_full_trajectory_data()


    def abort_check(self):
        pass
    
    def mission_abort(self):
        pass


    @staticmethod
    def soft_pow(v,p,eps):
        return (v**2 + eps**2)**(p/2)
    
    @staticmethod
    def soft_sign(v,eps):
        return v / (math.sqrt(v**2 + eps**2))

    def get_xyspeed_with_total_distance(self, total_dist):
        t_acc = self.max_vel_xy / self.a
        d1 = 0.5 * self.a * (t_acc ** 2)
        T1 = t_acc
        self.t += self.delta_T
        print(f"Dist: {total_dist:.2f}, t: {self.t:.2f}")
        if total_dist < self.point_accept_radius:
            Speed = self.min_vel_xy
        elif total_dist <= 2 * d1:
            if self.t <= math.sqrt(total_dist / self.a):
                Speed = self.a * self.t
            elif self.t <= 2 * math.sqrt(total_dist / self.a):
                Speed = self.a * (2 * math.sqrt(total_dist / self.a) - self.t)
            else:
                Speed = self.min_vel_xy
        else:
            T2 = (total_dist - 2 * d1) / self.max_vel_xy
            if self.t < T1:
                Speed = self.a * self.t
            elif T1 <= self.t <=(T2 + T1):
                Speed = self.max_vel_xy
            elif (T2 + T1)<=self.t <= 0.9*(2 * T1 + T2):
                Speed = self.max_vel_xy - self.a * (self.t - T2 - T1)
            else:
                Speed = self.min_vel_xy
        
        # === 新增：计算前方是否大角度转弯 ===
        if self.dest_index < self.number_of_points - 1:  # 不是最后一段
            try:
                p0 = self.localPosition[self.dest_index - 1]
                p1 = self.localPosition[self.dest_index]
                p2 = self.localPosition[self.dest_index + 1]

                v1 = np.array([p1[0]-p0[0], p1[1]-p0[1]])
                v2 = np.array([p2[0]-p1[0], p2[1]-p1[1]])

                cos_angle = np.dot(v1, v2) / (np.linalg.norm(v1)*np.linalg.norm(v2) + 1e-6)
                angle = math.acos(np.clip(cos_angle, -1.0, 1.0))
                angle_deg = math.degrees(angle)

                dist_to_dest = math.dist((self.now_x, self.now_y), (p1))

                if angle_deg > 30 and dist_to_dest < 5.0:
                    slow_factor = 0.4 + 0.6 * (math.pi - angle) / (math.pi - math.radians(30))
                    slow_factor *= (dist_to_dest / 5.0)
                    slow_factor = max(slow_factor, 0.3)
                    Speed *= slow_factor
            except Exception as e:
                print(f"[Speed] Angle calc failed: {e}")
        elif self.dest_index == self.number_of_points - 1:
        # 距离越近，速度越低；在 accept_radius 内线性降至 0
            if math.dist([self.now_x, self.now_y], [self.dest_x, self.dest_y]) <= self.point_accept_radius:
                Speed = self.min_vel_xy * (math.dist([self.now_x, self.now_y], [self.dest_x, self.dest_y]) / self.point_accept_radius)  

        return Speed

    def compute_target_distances(self):
        self.target_distances.clear()
        self.number_of_points = len(self.localPositions)
        for i in range(len(self.localPosition) - 1):
            p1 = self.localPosition[i]
            p2 = self.localPosition[i + 1]
            dist = math.dist(p1, p2)
            self.target_distances.append(dist)
    

    def speed_constraint(self, speed):
        return min(speed, self.max_vel_xy)

    def trans_to_localPositions(self):
        for i in range(len(self.datasets["latitudes"])):
            projected = self.publicResource.tools_resource.azimuthal_projection(
                self.datasets["latitudes"][i], self.datasets["longitudes"][i],
                self.publicResource.uav_resource.get_local_ref_lat(),
                self.publicResource.uav_resource.get_local_ref_lon())
            self.localPositions.append(projected)
            self.localPosition.append((projected[0], projected[1]))

    def get_los_setpoints(self, last_x, last_y, dest_x, dest_y, now_x, now_y, point_accept_radius):
        dy = dest_y - last_y
        dx = dest_x - last_x
        if dx == 0:
            cross_x = last_x
            cross_y = now_y
        else:
            k = dy / dx
            cross_x = (now_x + k ** 2 * last_x + k * now_y - k * last_y) / (1 + k ** 2)
            cross_y = last_y + k * (cross_x - last_x)

        dist_cn = math.dist((now_x, now_y), (cross_x, cross_y))
        if dist_cn >= point_accept_radius:
            los_x, los_y = cross_x, cross_y
        else:
            dist_cs = math.sqrt(point_accept_radius ** 2 - dist_cn ** 2)
            head_x = dest_x - cross_x
            head_y = dest_y - cross_y
            norm = math.sqrt(head_x ** 2 + head_y ** 2)
            los_x = cross_x + dist_cs * head_x / norm
            los_y = cross_y + dist_cs * head_y / norm

        return los_x, los_y, cross_x, cross_y
    
    def save_full_trajectory_data(self, save_dir="./mylog"):
        os.makedirs(save_dir, exist_ok=True)

        # 1. 规划路径 vs 实际路径
        filepath1 = os.path.join(save_dir, "planned_vs_actual_path.csv")
        with open(filepath1, 'w', newline='') as f:
            writer = csv.writer(f)
            writer.writerow(["type", "x_N", "y_E"])
            for x, y in self.planned_path:
                writer.writerow(["planned", x, y])
            for t, x, y in self.actual_trajectory:
                writer.writerow(["actual", x, y])
                
        print(f"[Saved] Planned vs actual path → {filepath1}")

        # 2. 横向误差 vs 时间（合并所有段）
        filepath2 = os.path.join(save_dir, "lateral_error_vs_time.csv")
        with open(filepath2, 'w', newline='') as f:
            writer = csv.writer(f)
            writer.writerow(["time_s", "lateral_error_m"])
            for seg_errors, seg_times in zip(self.errors, self.times):
                    for err_info, t in zip(seg_errors, seg_times):
                        writer.writerow([t, err_info['error']])
        print(f"[Saved] Lateral error vs time → {filepath2}")
